package star.crust.cartspace.framework;

import javax.vecmath.Matrix3d;

import star.core.util.geometry.DoubleXYZ;
import star.core.util.geometry.POTransform;

/**
 * Classe per la gestione dei box Aabb
 *
 */

public class CrtAabb extends CrtBoxing {

	private CrtMesh meshes;
	private DoubleXYZ center=null;
	private double[] extent=new double[3];

	public CrtAabb(CrtMesh meshes) {
		super(meshes.meshePrimitives, "AABB");

		this.meshes=meshes;
		DoubleXYZ min= new DoubleXYZ(Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY,Double.POSITIVE_INFINITY);
		DoubleXYZ max= new DoubleXYZ(Double.NEGATIVE_INFINITY,Double.NEGATIVE_INFINITY,Double.NEGATIVE_INFINITY);
		for (int i = 0; i != this.meshes.size(); ++i) 
			for (int j = 0; j != this.meshes.getPolygon().get(i).size(); ++j) {
				min.setInf(this.meshes.getPolygon().get(i).getVertex(j).dblposition());
				max.setSup(this.meshes.getPolygon().get(i).getVertex(j).dblposition());
			}
		min.setDivide(2);
		max.setDivide(2);
		this.center = min.setSum(max);
		this.extent=max.setMinus(min);

	}


	public DoubleXYZ getCenter()
	{
		return this.center;
	}

	public double[] getExtent()
	{
		return this.extent;
	}

	public int longestAxis()
	{
		return this.closestAxis(this.extent);
	}

	private int closestAxis(double[] vector)
	{
		double[] a= new double[2];
		a[0] = Math.abs(vector[0]);
		a[1] = Math.abs(vector[1]);
		int axis = a[0] < a[1] ? 1 : 0;
		return a[axis] < Math.abs(vector[2]) ? 2 : axis;
	}
	
	public double dblsize() { 
		return CrtAabbUtil.set_max((CrtAabbUtil.set_max(extent[0], extent[1])), extent[2]); }

	private static DoubleXYZ operator(DoubleXYZ p,POTransform t)
	{
		DoubleXYZ temp= new DoubleXYZ(t.getPosition().getX()+t.rowdot(0, p),
				t.getPosition().getY()+t.rowdot(1, p),
				t.getPosition().getZ()+t.rowdot(2, p));
		return temp;
	}

	public static boolean intersect(CrtAabb a,CrtAabb b, POTransform t, Matrix3d m)
	{
		DoubleXYZ p=CrtAabb.operator(b.getCenter(), t);
		return sep_axis_test(a.getExtent(), b.getExtent(), p.setMinusPoint(a.getCenter()),t.getOrientation(), m);
	}

	/**
	 * Metodo intersect tra i singoli box
	 */
	private static boolean sep_axis_test(double a[], double b[], DoubleXYZ pos, 
			Matrix3d orn, Matrix3d m) {

		if (a[0] + CrtAabbUtil.rowdot(m,0, b) < Math.abs(pos.getX())) return false;
		if (a[1] + CrtAabbUtil.rowdot(m,1, b) < Math.abs(pos.getY())) return false;
		if (a[2] + CrtAabbUtil.rowdot(m,2, b) < Math.abs(pos.getZ())) return false;

		if (CrtAabbUtil.dot(m,0,a) + b[0] < Math.abs(CrtAabbUtil.dot(orn, 0,pos.getVector() ))) return false;  
		if (CrtAabbUtil.dot(m,1,a) + b[1] < Math.abs(CrtAabbUtil.dot(orn, 1,pos.getVector() ))) return false;   
		if (CrtAabbUtil.dot(m,2,a) + b[2] < Math.abs(CrtAabbUtil.dot(orn, 2,pos.getVector() ))) return false;  

		if (a[1] * m.getElement(0,2) + a[2] * m.getElement(0,1) + b[1] * m.getElement(2,0) + b[2] * m.getElement(1,0) <  
				Math.abs(pos.getZ() * orn.getElement(0,1) - pos.getY() * orn.getElement(0,2)))
			return false;

		if (a[1] * m.getElement(1,2) + a[2] * m.getElement(1,1) + b[0] * m.getElement(2,0) + b[2] * m.getElement(0,0) <
				Math.abs(pos.getZ() * orn.getElement(1,1) - pos.getY() * orn.getElement(1,2)))
			return false;

		if (a[1] * m.getElement(2,2) + a[2] * m.getElement(2,1) + b[0] * m.getElement(1,0) + b[1] * m.getElement(0,0)<
				Math.abs(pos.getZ() * orn.getElement(2,1) - pos.getY() * orn.getElement(2,2)))
			return false;

		if (a[0] * m.getElement(0,2) + a[2] * m.getElement(0,0) + b[1] * m.getElement(2,1) + b[2] * m.getElement(1,1) <
				Math.abs(pos.getX() * orn.getElement(0,2) - pos.getZ() * orn.getElement(0,0)))
			return false;

		if (a[0] * m.getElement(1,2) + a[2] * m.getElement(1,0) + b[0] * m.getElement(2,1) + b[2] * m.getElement(0,1) < 
				Math.abs(pos.getX() * orn.getElement(1,2) - pos.getZ() * orn.getElement(1,0)))
			return false;

		if (a[0] * m.getElement(2,2) + a[2] * m.getElement(2,0) + b[0] * m.getElement(1,1) + b[1] * m.getElement(0,1) <
				Math.abs(pos.getX() * orn.getElement(2,2) - pos.getZ() * orn.getElement(2,0)))
			return false;

		if (a[0] * m.getElement(0,1) + a[1] * m.getElement(0,0) + b[1] * m.getElement(2,2) + b[2] * m.getElement(1,2) <
				Math.abs(pos.getY() * orn.getElement(0,0) - pos.getX() * orn.getElement(0,1)))
			return false;

		if (a[0] * m.getElement(1,1) + a[1] * m.getElement(1,0) + b[0] * m.getElement(2,2) + b[2] * m.getElement(0,2) <
				Math.abs(pos.getY() * orn.getElement(1,0) - pos.getX() * orn.getElement(1,1)))
			return false;

		if (a[0] * m.getElement(2,1) + a[1] * m.getElement(2,0) + b[0] * m.getElement(1,2) + b[1] * m.getElement(0,2) <
				Math.abs(pos.getY() * orn.getElement(2,0) - pos.getX() * orn.getElement(2,1)))     
			return false;

		return true;
	}

	public String toString() {
		return this.center.getX()+"\t"+this.center.getY()+"\t"+this.center.getZ()+"\t"+		extent[0]+"\t"+extent[1]+"\t"+extent[2]+"\n";
	}
	
	public CrtMesh getMeshes() {
		return meshes;
	}

	public float size() {
		return 0;
	}
	

}
